
/*第一个运动学模型*/

// 车轮半径参数 12cm
// 轮距 423mm
// 轴距 538mm

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "modbus/modbus.h"

#include "rclcpp/rclcpp.hpp"
#include "basic_sprayer_interfaces/msg/four_drivers.hpp" // 四个驱动
#include "basic_sprayer_interfaces/msg/four_wheel.hpp"   // 四轮速度
#include "basic_sprayer_interfaces/msg/adc.hpp"          // adc的Message
#include "basic_sprayer_interfaces/msg/human_cmd.hpp"    // 控制信号
#include "basic_sprayer_interfaces/msg/pid.hpp"          // 四轮速度

#include <math.h>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif // !M_PI


/* 将速度控制在100Hz  也就是每采样一次 转角运算一次速度 */

class FourValue
{
public:
    FourValue(float V_fl, float V_fr, float V_rl, float V_rr)
    {
        this->V_fr = V_fr;
        this->V_fl = V_fl;
        this->V_rl = V_rl;
        this->V_rr = V_rr;
    }
    float V_fr, V_fl, V_rl, V_rr;
};

class kineControler
{
public:
    kineControler()
    {
        pi_p = 0.06;
        pi_d = 0.0005;

        // pi_p = 0.06;
        // pi_d = 0.002;

        wheelTrack = 0.423;
        wheelBase = 0.538;
        wheelRadius = 0.12;

        speed_2_rpm = 60 / (0.12 * 2 * M_PI);

        dot_angle_front = 0;
        dot_angle_rear = 0;
    }

    void set_pd(float p,float d){
        pi_p = p;
        pi_d = d;
    }

    FourValue run(float target_speed, float target_deg);

    void setStatus(float angle_front_deg, float angle_rear_deg)
    {
        /* 可以计算d 做一个低通滤波 */
        // dot_angle_front = 0.5 * dot_angle_front + 0.5 * (angle_front - angle_front_deg) * 100;
        // dot_angle_rear = 0.5 * dot_angle_rear + 0.5 * (angle_rear - angle_rear_deg) * 100;

        dot_angle_front = (angle_front - angle_front_deg) * 100;
        dot_angle_rear = (angle_rear - angle_rear_deg) * 100;

        angle_front = angle_front_deg;
        angle_rear = angle_rear_deg;
    }

private:
    float wheelTrack;  // 轮距
    float wheelBase;   // 轴距
    float wheelRadius; // 车轮半径

    float speed_2_rpm;

    float angle_front; //前后转角
    float angle_rear;

    float dot_angle_front;
    float dot_angle_rear;

    float pi_p; // p参数
    float pi_d; // d参数
};
/**
* 
* @author 何思伟
* @param 
*
* @note 
*
* @return 
*/
FourValue kineControler::run(float target_speed, float target_deg)
{
    /*前后转向桥无约束*/
    float angele_rad = 0;
    float v_fl, v_fr, v_rl, v_rr;
    float front_error = target_deg - angle_front;
    float rear_error = -target_deg - angle_rear;

    float front_out = front_error * pi_p * wheelTrack / 2 + dot_angle_front * pi_d;
    float rear_out = rear_error * pi_p * wheelTrack / 2 + dot_angle_rear * pi_d;

    if (angle_front * angle_rear < 0) //不同符号
    {
        angele_rad = (angle_front - angle_rear) / 2 * M_PI / 180; //以前轮为准 后轮取反
        float rRadius = wheelBase / (2 * sin(angele_rad));
        float wheelTrack_rRadius = wheelTrack / (2 * rRadius);
        v_fl = target_speed * (1 - wheelTrack_rRadius) - front_out;
        v_fr = target_speed * (1 + wheelTrack_rRadius) + front_out;
        v_rl = target_speed * (1 - wheelTrack_rRadius) - rear_out;
        v_rr = target_speed * (1 + wheelTrack_rRadius) + rear_out;
    }
    else
    {
        v_fl = target_speed - front_out;
        v_fr = target_speed + front_out;
        v_rl = target_speed - rear_out;
        v_rr = target_speed + rear_out;
    }
    FourValue value(v_fl * speed_2_rpm, v_fr * speed_2_rpm, v_rl * speed_2_rpm, v_rr * speed_2_rpm);
    return value;
}

using namespace std::chrono_literals;

class kinematicsNode : public rclcpp::Node
{
public:
    kinematicsNode() : Node("kinematicsNode")
    {
        publisher_ = this->create_publisher<basic_sprayer_interfaces::msg::FourWheel>("driver/fourwheel", 10);
        /* 订阅adc */
        subscription_adc = this->create_subscription<basic_sprayer_interfaces::msg::Adc>(
            "adc", 10, std::bind(&kinematicsNode::callback_adc, this, std::placeholders::_1));
        /* 订阅车身状态 */
        subscription_driver = this->create_subscription<basic_sprayer_interfaces::msg::FourDrivers>(
            "driver/status", 10, std::bind(&kinematicsNode::callback_status, this, std::placeholders::_1));
        /*订阅发布的命令*/
        subscription_cmd = this->create_subscription<basic_sprayer_interfaces::msg::HumanCmd>(
            "/hesiwei/humancmd", 10, std::bind(&kinematicsNode::callback_cmd, this, std::placeholders::_1));

        subscription_pid = this->create_subscription<basic_sprayer_interfaces::msg::Pid>(
            "kinepid", 10, std::bind(&kinematicsNode::pid_callback, this, std::placeholders::_1));
        /*1s定时*/
        timer_ = this->create_wall_timer(1s, std::bind(&kinematicsNode::timer_callback, this));

        is_start = false;
        speed_target = 0;
        angle_front = 0;
        angle_rear = 0;

        comm_cnt = 0;
        controler = new kineControler();
        speed_target = 0;
    }

private:
    /*发布消息*/
    rclcpp::Publisher<basic_sprayer_interfaces::msg::FourWheel>::SharedPtr publisher_;

    rclcpp::Subscription<basic_sprayer_interfaces::msg::FourDrivers>::SharedPtr subscription_driver;
    rclcpp::Subscription<basic_sprayer_interfaces::msg::Adc>::SharedPtr subscription_adc;
    rclcpp::Subscription<basic_sprayer_interfaces::msg::HumanCmd>::SharedPtr subscription_cmd;

    rclcpp::Subscription<basic_sprayer_interfaces::msg::Pid>::SharedPtr subscription_pid;
    /*创建一个定时器*/
    rclcpp::TimerBase::SharedPtr timer_;

    bool is_start;
    float speed_target;
    float angle_target; // 目标角度

    float speed_target_raw; //

    float angle_front;
    float angle_rear;
    int8_t comm_cnt;

    int64_t timestamp; //时间戳

    kineControler *controler; // 控制器

    /*定时任务*/
    void timer_callback()
    {
        /*1s的时间内没有发生通信  直接停止运行*/
        if (comm_cnt == 0)
        {
            /* 发布消息让电机 转 */
            auto message = basic_sprayer_interfaces::msg::FourWheel();
            is_start = false;
            speed_target = 0;
            message.start_flag = 0;
            message.output_left_front = 0; //转换到rpm
            message.output_right_front = 0;
            message.output_left_rear = 0;
            message.output_right_rear = 0;
            publisher_->publish(message);
        }
        comm_cnt = 0;
    }

    void pid_callback(const basic_sprayer_interfaces::msg::Pid::SharedPtr msg)
    {
        float p = msg->p;
        float d = msg->d;
        RCLCPP_INFO(this->get_logger(), "p=%f,d=%f", p, d);
        controler->set_pd(p,d);
    }

    /*订阅adc*/
    void callback_adc(const basic_sprayer_interfaces::msg::Adc::SharedPtr msg)
    {
        angle_front = msg->angle_deg[0];
        angle_rear = msg->angle_deg[1];
        controler->setStatus(angle_front, angle_rear);
        /* 运行控制器 */
        if (is_start)
        {
            /*计算运行的时间dt*/
            /* 缓加 */
            const float step = 0.04; //0.02*60 = 1.2
            if (fabsf(speed_target_raw) < fabsf(speed_target))
            {
                speed_target = speed_target_raw;
            }
            else if (speed_target_raw - speed_target > step)
            {
                speed_target += step;
            }
            else if (speed_target_raw - speed_target < -step)
            {
                speed_target -= step;
            }
            else
            {
                speed_target = speed_target_raw;
            }

            FourValue value = controler->run(speed_target, angle_target);
            auto message = basic_sprayer_interfaces::msg::FourWheel();
            message.header.stamp = this->now();    // 给定时间
            message.start_flag = is_start ? 1 : 0;
            message.output_left_front = value.V_fl; //转换到rpm
            message.output_right_front = value.V_fr;
            message.output_left_rear = value.V_rl;
            message.output_right_rear = value.V_rr;
            publisher_->publish(message);
        }
        else
        {
            speed_target = 0;
            auto message = basic_sprayer_interfaces::msg::FourWheel();
            message.header.stamp = this->now();    // 给定时间
            is_start = false;
            message.start_flag = 0;
            message.output_left_front = 0; //转换到rpm
            message.output_right_front = 0;
            message.output_left_rear = 0;
            message.output_right_rear = 0;
            publisher_->publish(message);
        }
    }

    /**/
    void callback_status(const basic_sprayer_interfaces::msg::FourDrivers::SharedPtr msg)
    {

    }

    void callback_cmd(const basic_sprayer_interfaces::msg::HumanCmd::SharedPtr msg)
    {
        comm_cnt++;
        is_start = (msg->cmd_switch == msg->CMD_START);
        speed_target_raw = msg->velocity;
        // speed_target_raw = 0;
        angle_target = msg->angle;
    }
};

int main(int argc, char *argv[])
{
    /*创建 ros */
    rclcpp::init(argc, argv);
    auto pub_node = std::make_shared<kinematicsNode>();
    rclcpp::spin(pub_node);
    /*创建线程*/
    rclcpp::shutdown();
}
